{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "import matplotlib.pyplot as plt                  ### fig:robot_header\n",
    "import math                          #2-4行目を追加\n",
    "import matplotlib.patches as patches\n",
    "import numpy as np"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "class World:\n",
    "    def __init__(self):\n",
    "        self.objects = []             # ここにロボットなどのオブジェクトを登録\n",
    "        \n",
    "    def append(self,obj):             # オブジェクトを登録するための関数\n",
    "        self.objects.append(obj)\n",
    "    \n",
    "    def draw(self):\n",
    "        fig = plt.figure(figsize=(8,8))                # 8x8 inchの図を準備\n",
    "        ax = fig.add_subplot(111)                      # サブプロットを準備\n",
    "        ax.set_aspect('equal')                         # 縦横比を座標の値と一致させる\n",
    "        ax.set_xlim(-5,5)                              # X軸を-5m x 5mの範囲で描画\n",
    "        ax.set_ylim(-5,5)                              # Y軸も同様に\n",
    "        ax.set_xlabel(\"X\",fontsize=20)                 # X軸にラベルを表示\n",
    "        ax.set_ylabel(\"Y\",fontsize=20)                 # 同じくY軸に\n",
    "        \n",
    "        for obj in self.objects: obj.draw(ax)           # appendした物体を次々に描画\n",
    "            \n",
    "        plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "class IdealRobot:                                        # fig:idealrobot\n",
    "    def __init__(self, pose, color=\"black\"):\n",
    "        self.pose = pose        # 引数から姿勢の初期値を設定\n",
    "        self.r = 0.2            # これは描画のためなので固定値\n",
    "        self.color = color      # 引数から描画するときの色を設定\n",
    "        \n",
    "    def draw(self, ax):\n",
    "        x, y, theta = self.pose                   # 姿勢の変数を分解して3つの変数へ\n",
    "        xn = x + self.r * math.cos(theta)         #  ロボットの鼻先のx座標 \n",
    "        yn = y + self.r * math.sin(theta)         #  ロボットの鼻先のy座標 \n",
    "        ax.plot([x,xn], [y,yn], color=self.color) # ロボットの向きを示す線分の描画\n",
    "        c = patches.Circle(xy=(x, y), radius=self.r, fill=False, color=self.color) \n",
    "        ax.add_patch(c)   # 上のpatches.Circleでロボットの胴体を示す円を作ってサブプロットへ登録"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "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\n",
      "text/plain": [
       "<Figure size 576x576 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "world = World()            ### fig:execution_robot_class\n",
    "# world.draw()         # これは削除\n",
    "### 以下追加 ###\n",
    "robot1 = IdealRobot( np.array([2, 3, math.pi/6]).T )           # ロボットのインスタンス生成（色を省略）\n",
    "robot2 = IdealRobot( np.array([-2, -1, math.pi/5*6]).T, \"red\")  # ロボットのインスタンス生成（色を指定）\n",
    "world.append(robot1)                                      # ロボットを登録 \n",
    "world.append(robot2)\n",
    "world.draw()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.7.3"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
